#ifndef FORWARD_KINEMATICS_H
#define FORWARD_KINEMATICS_H

#include <stdint.h>

/**
 * @brief 根据理想 DH 模型计算正运动学（Matlab 版本）
 *
 * 输入：
 *  - theta1, theta5, theta6 : 角度，单位 °
 *  - d2, d3, d4             : 直线位移，单位 m
 *  - obj_pos[6]             : 工具/管尖到末端的变换（同 Matlab 约定）
 *                             obj_pos[0..2] = x,y,z (单位：μm)
 *                             obj_pos[3..5] = rx,ry,rz (单位：°)
 *
 * 输出（全部通过指针返回）：
 *  - out_x_um, out_y_um, out_z_um : 末端在基坐标系下的位置（单位：μm）
 *  - out_alpha_deg, out_beta_deg, out_gamma_deg : 末端姿态，单位 °
 *
 * 说明：该函数实现与 Matlab 中 ideal_DH_FK 等价的流程：
 *   1) 先用 theta/d 构造 T_e0（机械臂末端到基的变换）
 *   2) 用 obj_pos 构造 T_Te（工具到末端的变换）
 *   3) 计算 T_T0 = T_e0 * T_Te （工具相对于基的变换）
 *   4) 从 T_T0 提取姿态与位置（姿态返回顺序与 Matlab 一致）
 */
void ideal_DH_FK(double theta1_deg, double d2_m, double d3_m, double d4_m,
                 double theta5_deg, double theta6_deg, const double obj_pos[6],
                 double *out_x_um, double *out_y_um, double *out_z_um,
                 double *out_alpha_deg, double *out_beta_deg,
                 double *out_gamma_deg);

/**
 * @brief 根据各关节电机脉冲数计算正运动学（使用 Matlab 的输出约定）
 *
 * 输入：
 *  - pulse_* : 各轴脉冲
 *  - obj_pos[6] : 工具偏置（单位同上，μm/°）
 *
 * 输出：
 *  - out_x_um, out_y_um, out_z_um : 末端位置（μm）
 *  - out_alpha_deg, out_beta_deg, out_gamma_deg : 姿态（°）
 *
 * 说明：
 *  - 将脉冲 -> 角度/位移 -> 调用 ideal_DH_FK_matlab 得到最终结果（单位同
 * Matlab）
 *  - 注意 theta6 的脉冲可能来自“光栅（grating）”或者“电机编码器”，本实现使用
 * Matlab FK 文件里
 *    用的“光栅”换算比例（见实现中的注释），如需改回另一组比率请在实现中调整常量。
 */
void forward_kinematics_from_pulses(int pulse_theta1, int pulse_d2,
                                    int pulse_d3, int pulse_d4,
                                    int pulse_theta5, int pulse_theta6,
                                    const double obj_pos[6], double *out_x_um,
                                    double *out_y_um, double *out_z_um,
                                    double *out_alpha_deg, double *out_beta_deg,
                                    double *out_gamma_deg);

#endif // FORWARD_KINEMATICS_H
